#ifndef __SV2_MOT__
#define __SV2_MOT__

#include "sms_core.h"
#include <string>
#include <chrono>
#include <mutex>
#include <queue>
#include <condition_variable>
#include <Eigen/Dense>
//#include "sort.h"
#include "BYTETracker.h"


namespace sv2
{

 // class SORT;
  class BYTETracker;

  class MultipleObjectTrackerNode : public sms::BaseNode
  {
  public:
    MultipleObjectTrackerNode(
        std::string job_name,
        std::string param_file,
        std::string ip = "127.0.0.1",
        int port = 9094) : sms::BaseNode("MultipleObjectTrackerNode", job_name, param_file, ip, port),
                           _det_res_sub("/" + job_name + "/detector/image_results", "sensor_msgs::CompressedImage", std::bind(&MultipleObjectTrackerNode::full_res_callback, this, std::placeholders::_1)),
                           _res_pub("/" + job_name + "/mot/results", "spirecv_msgs::2DTargets"),
                           _res_img_pub("/" + job_name + "/mot/image_results", "sensor_msgs::CompressedImage")
    {
      // 读取节点参数
      this->_algorithm = this->get_param("algorithm", "bytetrack");
      this->_reid_input_h = this->get_param("reid_input_h", 128);
      this->_reid_input_w = this->get_param("reid_input_w", 128);
      this->_reid_num_samples = this->get_param("reid_num_samples", 10);
      this->_reid_match_thres = this->get_param("reid_match_thres", 2.0);
      this->_iou_thres = this->get_param("iou_thres", 0.5);
      this->_max_age = this->get_param("max_age", 10);
      this->_min_hits = this->get_param("min_hits", 3);
      this->_frame_rate = this->get_param("frame_rate", 30);
      this->_track_buffer = this->get_param("track_buffer", 30);
      this->_mot_id = this->get_param("mot_id", 0);

      logger.info("algorithm: " + this->_algorithm);
      logger.info("mot_id: " + std::to_string(static_cast<int>(this->_mot_id)));
      if ("bytetrack" == this->_algorithm)
      {
        logger.info("frame_rate: " + std::to_string(static_cast<int>(this->_frame_rate)));
        logger.info("track_buffer: " + std::to_string(static_cast<int>(this->_track_buffer)));
        this->_bytetrack_impl = new BYTETracker(this->_frame_rate, this->_track_buffer);
      }
      else if ("sort" == this->_algorithm)
      {
        logger.info("reid_input_h: " + std::to_string(static_cast<int>(this->_reid_input_h)));
        logger.info("reid_input_w: " + std::to_string(static_cast<int>(this->_reid_input_w)));
        logger.info("reid_num_samples: " + std::to_string(static_cast<int>(this->_reid_num_samples)));
        logger.info("reid_match_thres: " + std::to_string(static_cast<double>(this->_reid_match_thres)));
        logger.info("iou_thres: " + std::to_string(static_cast<float>(this->_iou_thres)));
        logger.info("max_age: " + std::to_string(static_cast<int>(this->_max_age)));
        logger.info("min_hits: " + std::to_string(static_cast<int>(this->_min_hits)));
        //this->_sort_impl = new SORT(this->_iou_thres, this->_max_age, this->_min_hits);
      }
    }
    ~MultipleObjectTrackerNode()
    {
    }
    void run();

    void full_res_callback(nlohmann::json msg)
    {
      // 放入到阻塞队列中
      {
        std::unique_lock<std::mutex> lock(this->_full_queue_mtx);
        this->_full_queue.push(msg);
      }
      // 通知主线程取数据
      this->_full_cv.notify_one();
    }

    std::string _algorithm;
    bool _with_reid;
    int _reid_input_h;
    int _reid_input_w;
    int _reid_num_samples;
    double _reid_match_thres;
    double _iou_thres;
    int _max_age;
    int _min_hits;
    int _frame_rate;
    int _track_buffer;
    int _mot_id;

    //SORT *_sort_impl;
    BYTETracker *_bytetrack_impl;

  private:
    // 订阅话题
    sms::Subscriber _det_res_sub;
    // 发布话题
    sms::Publisher _res_pub;
    sms::Publisher _res_img_pub;

    std::mutex _full_queue_mtx;
    std::queue<nlohmann::json> _full_queue;
    std::condition_variable _full_cv;


};

}
#endif
